#ifndef SLAM_ROS_slam_ROS_PUBLISH_OCCUPANCY_GRID_MAP_H
#define SLAM_ROS_slam_ROS_PUBLISH_OCCUPANCY_GRID_MAP_H

#include <cmath>
#include <string>
#include <thread>
#include <vector>
#include <memory>
#include <mutex>

#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cairo/cairo.h"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "slam/common/port.h"
#include "slam/io/image.h"
#include "slam/io/submap_painter.h"
#include "slam/mapping/id.h"
#include "slam/transform/rigid_transform.h"
#include "slam_ros/map_server_2d.h"
#include "slam_ros/msg_conversion.h"
#include "slam_ros/node_constants.h"
#include "slam_ros/ros_log_sink.h"
#include "slam_ros/ros_map.h"
#include "slam_ros/submap.h"
#include "slam_ros_msgs/srv/get_map_data.hpp"
#include "slam_ros_msgs/srv/query_slam_status.hpp"
#include "slam_ros_msgs/msg/submap_list.hpp"
#include "slam_ros_msgs/srv/submap_query.hpp"
#include "agv_srvs/srv/get_map_point.hpp"
#include "agv_msgs/msg/map_png.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/service.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/timer.hpp"

namespace slam_ros {

using ::slam::io::PaintSubmapSlicesResult;
using ::slam::io::SubmapSlice;
using ::slam::mapping::SubmapId;

class PublishOccupancyGridMap {
 public:
  explicit PublishOccupancyGridMap(std::shared_ptr<rclcpp::Node> node,
                                  const double resolution,
                                  const int map_save_time_interval);
  ~PublishOccupancyGridMap() = default;

  PublishOccupancyGridMap(const PublishOccupancyGridMap&) = delete;
  PublishOccupancyGridMap& operator=(const PublishOccupancyGridMap&) = delete;

  void HandleGetOccupancyGrid(
      const std::shared_ptr<rmw_request_id_t> request_header,
      const std::shared_ptr<nav_msgs::srv::GetMap::Request> request,
      const std::shared_ptr<nav_msgs::srv::GetMap::Response> response);

  void HandleGetMapPoint(
      const std::shared_ptr<rmw_request_id_t> request_header,
      const std::shared_ptr<agv_srvs::srv::GetMapPoint::Request> request,
      const std::shared_ptr<agv_srvs::srv::GetMapPoint::Response> response);

  void HandleGetMapData(
      const std::shared_ptr<rmw_request_id_t> request_header,
      const std::shared_ptr<slam_ros_msgs::srv::GetMapData::Request> request,
      const std::shared_ptr<slam_ros_msgs::srv::GetMapData::Response> response);

  void GetMapResponse(std::shared_ptr<nav_msgs::srv::GetMap::Response> map_data_response);

  void SaveMappingMap();

 private:
  void HandleSubmapList(const slam_ros_msgs::msg::SubmapList::SharedPtr msg);

  void GetMappingMap(std::shared_ptr<nav_msgs::srv::GetMap::Response> map_data_response);

  std::string base64Encode(const unsigned char* Data, int DataByte);

  const double resolution_;
  const int map_save_time_interval_;
  const bool include_frozen_submaps_ = true;
  const bool include_unfrozen_submaps_ = true;
  const std::string map_frame_id_ = "map";

  std::mutex mutex_;
  rclcpp::Client<slam_ros_msgs::srv::SubmapQuery>::SharedPtr submap_query_client_;
  rclcpp::Subscription<slam_ros_msgs::msg::SubmapList>::SharedPtr submap_list_subscriber_;
  std::map<SubmapId, SubmapSlice> submap_slices_ GUARDED_BY(mutex_);
  std::string last_frame_id_;
  builtin_interfaces::msg::Time last_timestamp_;
  rclcpp::Client<slam_ros_msgs::srv::QuerySlamStatus>::SharedPtr query_slamstatus_client_;
  rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr get_occupancy_map_service_;
  rclcpp::Service<slam_ros_msgs::srv::GetMapData>::SharedPtr get_map_data_service_;
  rclcpp::Service<agv_srvs::srv::GetMapPoint>::SharedPtr get_map_point_service_;
  slam_ros::MapServer2D map_server_2d_;
  rclcpp::Publisher<agv_msgs::msg::MapPng>::SharedPtr map_publisher_;

  slam_ros::SlamType slam_type_ = slam_ros::SlamType::UNKNOWN;
  rclcpp::TimerBase::SharedPtr save_map_timer_;
  std::shared_ptr<rclcpp::Node> node_;
};

}  // namespace slam_ros

#endif